#pragma once

#include "./track.cpp"
#include "./imgprocess.cpp"
#include "./control.cpp"
#include "./motion.cpp"
#include "./circle.cpp"
#include "../include/uart.hpp"
#include "../include/common.hpp"     //公共类方法文件
#include <opencv2/highgui.hpp>
#include <opencv2/opencv.hpp>
#include <opencv2/imgproc.hpp>


#include "../include/detection.hpp"  //百度Paddle框架移动端部署
#include "./rescue.cpp"
#include "./danger.cpp"
#include "./zebracrossing.cpp"

#include <thread>
#include <mutex>
#include "./img_thread.cpp"

using namespace std;
using namespace cv;




int main()
{
    VideoCapture capture;       // Opencv相机类
    imgprocess imgprocess;      //使用传参的形式传递数组，不用全局变量
    track track;             //边线需要传递，使用别名传递 因为在类里面写的函数不认他
    control control;
    Motion motion;
    Circle circle;
    Scene scene=NormalScene;
    
    Rescue rescue;
    Danger danger;
    Parking park;

    mutex mtx_img;//线程锁

    string save_path="/24_projects/video_save/danger.mp4";
    VideoWriter outputVideo;
    outputVideo.open(motion.params.video,CV_FOURCC('D', 'I', 'V', 'X'), 30.0, Size(640, 480));



//摄像头
    capture = VideoCapture("/dev/video0"); // 打开摄像头
    if (!capture.isOpened()) 
    {
        printf("can not open video device!!!\n");
        return 0;
    }
    capture.set(CAP_PROP_FRAME_WIDTH, 640);  // 设置图像分辨率
    capture.set(CAP_PROP_FRAME_HEIGHT, 480); // 设置图像分辨率
    capture.set(CV_CAP_PROP_FOURCC, CV_FOURCC('M', 'J', 'P', 'G'));
    capture.set(CV_CAP_PROP_FPS, 120);//帧率 帧/秒
    //摄像头硬件缺陷导致320*240读取只有50帧，640*320的120帧正常
    Mat frame;

//上下机位通信
    shared_ptr<Uart> uart = make_shared<Uart>("/dev/ttyUSB0"); // 初始化串口驱动
    int ret = uart->open();
    if (ret != 0) {
        printf("[Error] Uart Open failed!\n");
        return -1;
    }
    uart->startReceive(); // 启动数据接收子线程
    //智能指针里uart；uart里面套智能指针；智能指针里面套thread;
    

    // 目标检测类(AI模型文件)
    shared_ptr<Detection> detection = make_shared<Detection>(motion.params.model);
    detection->score = motion.params.score; // AI检测置信度
    Mat detection_img; //如果将track的img传过去会导致段错误（不知道为什么）
    //1.双摄像头，双摄像头同时申请的带宽太大总线带宽会被占满
    //2.ai的多线程 现在的双线程只是将摄像头和ai分开
    img_thread detection_thread;
    detection_thread.init(motion);

    long preTime;
    float speed;
    while(1)
    {
        uart->carControl(0, PWMSERVOMID); // 通信控制车辆停止运动
        while (!uart->keypress)
        {
            waitKey(300);
        }
        uart->keypress=false;
        uart->buzzerSound(uart->BUZZER_START); // 祖传提示音效
        waitKey(1000);
        if(uart->keypress)
        {
            uart->carControl(0, PWMSERVOMID); // 通信控制车辆停止运动
            uart->buzzerSound(uart->BUZZER_FINISH); // 祖传提示音效
            break;
        }
        uart->buzzerSound(uart->BUZZER_OK); // 祖传提示音效
        scene=NormalScene;//场景置0
        danger.init();

    while(1)
    {
        speed=motion.params.speedHigh;//速度
auto startTime = chrono::duration_cast<chrono::milliseconds>(chrono::system_clock::now().time_since_epoch()).count();//帧率计算
printf(">>main FrameTime: %ldms | %.2ffps \n", startTime - preTime,1000.0 / (startTime - preTime));
preTime = chrono::duration_cast<chrono::milliseconds>(chrono::system_clock::now().time_since_epoch()).count();//程序开始时间

//20ms
        if(!capture.read(frame))//获取图片
        {continue;}
//8ms
        outputVideo.write(frame);

        detection_img = frame.clone();//复制一张img
        detection->inference(detection_img);
        resize(frame, track.img, Size(COLSIMAGE,ROWSIMAGE), 0, 0, INTER_NEAREST);
        track.binary_mat = imgprocess.binaryzation(track.img); // 图像二值化
        imgprocess.mat_to_arry(track);//mat和arry都在track里面
        track.find_mid_line();//找边线岔点

        switch(scene)
        {
        case NormalScene://上一帧无场景，判断这一帧场景
            if(rescue.if_rescue(detection->results)!=0&&motion.params.rescue==true&&motion.params.cross==true)
            {
                scene=RescueScene;
                break;
            }else if(danger.if_danger(detection->results)!=0&&motion.params.danger==true&&motion.params.cross==true)
            {
                scene=DangerScene;
                break;
            }else if(park.process(detection->results)!=0&&motion.params.parking==true)
            {
                scene=ParkingScene;
            }else if(circle.RoadIsRoundabout(track)!=0&&motion.params.ring==true)//圆环
            {
                scene=RingScene;
                // uart->carControl(0, PWMSERVOMID); // 通信控制车辆停止运动
                // uart->keypress=true;
                break;
            }
        break;
        case RingScene://上一帧是圆环，对圆环处理，
            circle.RoundaboutProcess(track);
            if(circle.state==0)//如果从圆环中出来了回到无场景
            {scene=Scene::NormalScene;}
        break;
        case RescueScene:
            speed=motion.params.speedLow;
            rescue.process(track,detection->results,false);
            if(rescue.step==0){
                rescue.rescue_init();
                scene=Scene::NormalScene;
            }else if(rescue.step==5){
                speed=0;
            }else if(rescue.step==6){
                speed=(-1*motion.params.speedLow);
            }
            //停不准的话加上偏差值的判断
        break;
        case DangerScene:
            speed=motion.params.speedLow;
            //判断result是否正在被写入
            if(danger.process(track,detection->results)==1)
            {scene=Scene::NormalScene;}
        break;
        case ParkingScene:
            if(park.process(detection->results)==true)
            {
                break;
            }
        }
        control.get_devation(track);//算偏差
        motion.poseCtrl(control.devation);//偏差算舵机pwm



        if(motion.params.debug){
            // uart->carControl(0, motion.servoPwm);//debug==true的时候车不动        
            imgprocess.display_window(park,danger,rescue,circle,motion,control, track ,"find lines stest");//展示单张图像
            
if(motion.params.cross)
{
            detection->drawBox(detection_img); // 图像绘制AI结果
            imshow("ai_detection", detection_img);
            waitKey(10);    // 等待显示
}

        }else{
            // uart->carControl_thread(speed, motion.servoPwm);//传到下位机
        }
        if(uart->keypress)
        {
            uart->keypress=false;
            waitKey(100);
            uart->carControl(0, PWMSERVOMID); // 通信控制车辆停止运动
            break;
        }

    }
    }
    capture.release();
    // uart->close(); // 串口通信关闭
	outputVideo.release();

    return 0;
}

